#include "System_Task.h"
#include "cmsis_os.h"
#include "main.h"
#include "stdio.h"
#include "freertos.h"
#include "remote_control.h"
#include "Chassis_Task.h"
#include "Arm_Task.h"

System_Iifo_Typedef System_Info;

static void Mode_Judeg(System_Iifo_Typedef *System);
static void CAN_Send(void);

int GetTick;
float tim;
int Pump_Change[2];
bool Handoff;

void System_Task(void const *argument)
{
  /* USER CODE BEGIN System_Task */
  /* Infinite loop */
  for (;;)
  {
    Mode_Judeg(&System_Info);
    HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, Arm_Info.Pump);
    CAN_Send();
    tim = 1000.f / (HAL_GetTick() - GetTick);
    GetTick = HAL_GetTick();

    osDelay(1);
  }
  /* USER CODE END System_Task */
}

void Mode_Judeg(System_Iifo_Typedef *System)
{
  switch (remote_ctrl.rc.s[1])
  {
  case 1:
  {
    switch (remote_ctrl.rc.s[0])
    {
    case 2:
    {
      //Arm_Info.Pump = 0;
      Arm_Info.Mode = KEEP;
      Chassis_Info.Mode = Lock;
    }
    break;

    case 3:
    {
     // Arm_Info.Pump = 1;
      Arm_Info.Mode = KEEP;
      Chassis_Info.Mode = Lock;
    }
    break;

    case 1:
    {
     // Arm_Info.Pump = 1;
      Arm_Info.Mode = KEEP;
      Chassis_Info.Mode = Lock;
    }
    break;
    }
  }
  break;

  case 3:
  {

    switch (remote_ctrl.rc.s[0])
    {
    case 2:
    {
      //Arm_Info.Pump = 0;
      Arm_Info.Mode = STANDBY;
      Chassis_Info.Mode = Sync;
    }
    break;

    case 3:
    {
      //Arm_Info.Pump = 1;
      Arm_Info.Mode = KEEP;
      Chassis_Info.Mode = Lock;

      if (Pump_Change[0] && Pump_Change[1])
      {
        if (Arm_Info.Pump == 0)
          Arm_Info.Pump = 1;
        else
          Arm_Info.Pump = 0;
        Pump_Change[1] = 0;
      }
      else if (!Pump_Change[0])
        Pump_Change[1] = 1;

        Pump_Change[0] = 0;
    }
    break;

    case 1:
    {
     // Arm_Info.Pump = 1;
      Arm_Info.Mode = STANDBY;
      Chassis_Info.Mode = Sync;

      Pump_Change[0] = 1;
    }
    break;
    }

    // Shoot_Info.Fire.Change=1;
  }
  break;
  case 2:
  {

    Arm_Info.Pump = 0;
    Arm_Info.Mode = OFF;
    Chassis_Info.Mode = OFF;
	Pump_Change[0] = 0;
  }
  break;
  }

  if (remote_ctrl.rc_lost == 1)
  {
    Arm_Info.Pump = 0;
    Arm_Info.Mode = OFF;
    Chassis_Info.Mode = OFF;
	  
	 Pump_Change[0] = 0;
  }
}

void CAN_Send(void)
{

  if (Handoff == 0)
  {
    USER_CAN_TxMessage(Arm_Motor[0][0].CANx, Arm_Motor[0][0].CANFrame.TxStdId, Arm_Info.Send.S[0], 8);
    USER_CAN_TxMessage(Arm_Motor[0][1].CANx, Arm_Motor[0][1].CANFrame.TxStdId, Arm_Info.Send.S[1], 8);
    USER_CAN_TxMessage(Arm_Motor[0][2].CANx, Arm_Motor[0][2].CANFrame.TxStdId, Arm_Info.Send.S[2], 8);
    Handoff = 1;
  }
  else
  {
    USER_CAN_TxMessage(Arm_Motor[1][0].CANx, Arm_Motor[1][0].CANFrame.TxStdId, Arm_Info.Send.B[0], 8);
    USER_CAN_TxMessage(Arm_Motor[1][1].CANx, Arm_Motor[1][1].CANFrame.TxStdId, Arm_Info.Send.B[1], 8);
    Handoff = 0;
  }

  USER_CAN_TxMessage(Arm_Motor[1][2].CANx, Arm_Motor[1][2].CANFrame.TxStdId, Arm_Info.Send.B[2], 8);

  //  USER_CAN_TxMessage(CAN2,0xA0, Arm_Info.Send.S[0], 8);
  //  USER_CAN_TxMessage(CAN2,0xA1, Arm_Info.Send.S[1], 8);
  //  USER_CAN_TxMessage(CAN2,0xA2, Arm_Info.Send.S[2], 8);
  //    osDelay(1);

  //  USER_CAN_TxMessage(CAN2,0xB0, Arm_Info.Send.B[0], 8);
  //  USER_CAN_TxMessage(CAN2,0xB1, Arm_Info.Send.B[1], 8);
  //  USER_CAN_TxMessage(CAN1,0xB2, Arm_Info.Send.B[2], 8);
  //

  Chassis_Motor[0].CANFrame.TxData[0] = (uint8_t)(Chassis_Info.Output[LF] >> 8);
  Chassis_Motor[0].CANFrame.TxData[1] = (uint8_t)(Chassis_Info.Output[LF]);
  Chassis_Motor[0].CANFrame.TxData[2] = (uint8_t)(Chassis_Info.Output[LB] >> 8);
  Chassis_Motor[0].CANFrame.TxData[3] = (uint8_t)(Chassis_Info.Output[LB]);
  Chassis_Motor[0].CANFrame.TxData[4] = (uint8_t)(Chassis_Info.Output[RB] >> 8);
  Chassis_Motor[0].CANFrame.TxData[5] = (uint8_t)(Chassis_Info.Output[RB]);
  Chassis_Motor[0].CANFrame.TxData[6] = (uint8_t)(Chassis_Info.Output[RF] >> 8);
  Chassis_Motor[0].CANFrame.TxData[7] = (uint8_t)(Chassis_Info.Output[RF]);
  USER_CAN_TxMessage(Chassis_Motor[0].CANx, Chassis_Motor[0].CANFrame.TxStdId, Chassis_Motor[0].CANFrame.TxData, 8);
}